#include <boost/thread/mutex.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/common/time.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/registration/icp.h>
#include <sstream>

using namespace pcl;
using namespace std;

// I can't figure out the syntax of boost::bind() so I'm going to be lazy and
// use a global variable and function.
bool capture = false, done_capturing = false;
boost::mutex keyboard_mutex;
void keyboardEventOccurred (const visualization::KeyboardEvent &event, void* viewer_void) {
  keyboard_mutex.lock();
  if (event.getKeySym() == "n") {
    capture = true;
  }
  if (capture && event.getKeySym() == "m") {
    capture = false;
    done_capturing = true;
    cout << "Done capturing" << endl;
  }
  keyboard_mutex.unlock();
}

class SimpleKinectViewer {
public:
  SimpleKinectViewer () : viewer ("PCL Kinect Viewer"), aligning_(false) {}
  void cloud_cb_ (const PointCloud<PointXYZRGBA>::ConstPtr &cloud) {
    if (aligning_) {
      return;
    } else {
      boost::mutex::scoped_lock lock(cloud_mutex_);
      PointCloud<PointXYZRGBA>::Ptr pass_cloud(new PointCloud<PointXYZRGBA>);
      PassThrough<PointXYZRGBA> pass;
      pass.setInputCloud(cloud);
      pass.setFilterFieldName("z");
      pass.setFilterLimits(0, 2.0);
      pass.filter(*pass_cloud);
      
      PointCloud<PointXYZRGBA>::Ptr voxel_cloud(new PointCloud<PointXYZRGBA>);
      VoxelGrid<PointXYZRGBA> vg;
      vg.setInputCloud (pass_cloud);
      vg.setLeafSize (0.01f, 0.01f, 0.01f);
      vg.filter (*voxel_cloud);
      
      if (!done_capturing) {
	viewer.showCloud(voxel_cloud, "first cloud");
	if (capture) {
	  cloud_list_.push_back(voxel_cloud);
	  stringstream ss;
	  ss << "bun" << iter << ".pcd";
	  io::savePCDFile(ss.str(), *voxel_cloud);
	  cout << "Saved cloud in " << ss.str() << "\n";
	  iter++;
	}
      } else {
	alignClouds();
      }
    }
  }

  void alignClouds() {
    aligning_ = true;

    Eigen::Matrix4f guess;
    guess.setIdentity();
    PointCloud<PointXYZRGBA>::ConstPtr first_cloud = cloud_list_.front();
    viewer.showCloud(cloud_list_.back(), "first cloud");
    int i = 0;
    PointCloud<PointXYZRGBA>::Ptr result;
    for (typename list<PointCloud<PointXYZRGBA>::Ptr>::const_iterator it = cloud_list_.begin();
	 it != cloud_list_.end(); ++it, ++i) {
      cout << "Aligning iteration " << i << " / " << cloud_list_.size() << endl;
      PointCloud<PointXYZRGBA>::ConstPtr current_cloud = *it;
      IterativeClosestPoint<PointXYZRGBA, PointXYZRGBA> icp;
      icp.setInputCloud(first_cloud);
      icp.setInputTarget(current_cloud);
      result.reset(new PointCloud<PointXYZRGBA>);
      icp.align(*result, guess);
      guess = icp.getFinalTransformation();
      viewer.showCloud(result, "result cloud");
    }
    cout << "Done aligning. Result is:" << endl;
    cout << guess << endl;
    ofstream output("matrix.txt", ofstream::out);
    output << guess << endl;
    output.close();
    cout << "Wrote matrix to matrix.txt" << endl;
    
    io::savePCDFile("result.pcd", *result);
    cout << "Resulting point cloud written to result.pcd" << endl;
  }

  void run (char* dev_id) {
    iter = 0;
    capture = false;
    Grabber* interface = new OpenNIGrabber(dev_id);

    boost::function<void(const PointCloud<PointXYZRGBA>::ConstPtr&)> f =
      boost::bind(&SimpleKinectViewer::cloud_cb_, this, _1);

    interface->registerCallback(f);
    viewer.registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);

    interface->start();
    while (!viewer.wasStopped()) {
      sleep(1);
    }
    interface->stop();
  }

  visualization::CloudViewer viewer;
  list<PointCloud<PointXYZRGBA>::Ptr> cloud_list_;
  int iter;
  bool aligning_;
  boost::mutex cloud_mutex_;
};

int main (int argc, char** argv) {
  if (argc < 2) {
    cout << "Usage: ./calibration serial_id" << endl;
    return -1;
  }
  SimpleKinectViewer v;
  v.run (argv[1]);
  return 0;
}

